/**
 ******************************************************************************
 * @file    arm_air.c
 * @author  GEEKROS,  site:www.geekros.com
 ******************************************************************************
 */
 
 #include "arm_air.h"

// 实例化电机数据结构
Arm_Motor_Struct Motor_Data[CAN_MOTOR_LEN];

// 电机上电状态
int Motor_Status = 0;

// 电机校准状态
int Motor_Calibration_Status = 0;

/******************************************************************
  * @ 函数名  ： Arm_Init
  * @ 功能说明： 机械臂初始化
  * @ 参数    ： NULL
  * @ 返回值  ： NULL
  *****************************************************************/
void Arm_Init(void)
{
	// 打开开发板24V可控电源
	Power_State(24, 0, "on");
    
    // 关闭低电量报警
    Adc_Low_Voltage(0);
    
    // 初始化电机校准偏差值
    //Motor_Data[5].Offset = 145.00f;
    
    // 所有电机上电
    //Arm_Motor_Start();
}

/******************************************************************
  * @ 函数名  ： Arm_Format_Send_Data
  * @ 功能说明： 格式化发送数据
  * @ 参数    ： NULL
  * @ 返回值  ： NULL
  *****************************************************************/
int16_t Arm_Format_Send_Data(int Data)
{
    int16_t Return_Data;
    
    Return_Data = ((int16_t)Data);
    
    return Return_Data;
}

/******************************************************************
  * @ 函数名  ： Arm_Motor_Start
  * @ 功能说明： 所有电机上电
  * @ 参数    ： NULL
  * @ 返回值  ： NULL
  *****************************************************************/
void Arm_Motor_Start(void)
{
    delay_ms(500);
    Can_Send(0x0AA, Arm_Format_Send_Data(1), Arm_Format_Send_Data(1), Arm_Format_Send_Data(1), Arm_Format_Send_Data(1));
    Motor_Status = 1; // 更改电机上电状态
    Led_State("green", "on");
    delay_ms(500);
}


/******************************************************************
  * @ 函数名  ： Arm_Motor_Stop
  * @ 功能说明： 所有电机下电
  * @ 参数    ： NULL
  * @ 返回值  ： NULL
  *****************************************************************/
void Arm_Motor_Stop(void)
{
    Can_Send(0x0AA, Arm_Format_Send_Data(0), Arm_Format_Send_Data(0), Arm_Format_Send_Data(0), Arm_Format_Send_Data(0));
    delay_ms(200);
    Motor_Status = 0; // 更改电机上电状态
    Led_State("green", "off");
}

/******************************************************************
  * @ 函数名  ： Arm_Motor_Motion_Format
  * @ 功能说明： 电机运动控制数据格式化
  * @ 参数    ： NULL
  * @ 返回值  ： NULL
  *****************************************************************/
void Arm_Motor_Motion_Format(void)
{
    for (int i = 0; i < CAN_MOTOR_LEN; i++)
    {
        if(i == 0 || i == 1)
        {
            Arm_Motor_Motion(0x1AA, Arm_Format_Send_Data(Motor_Data[0].Position), Arm_Format_Send_Data(Motor_Data[0].Speed), Arm_Format_Send_Data(Motor_Data[1].Position), Arm_Format_Send_Data(Motor_Data[1].Speed));
            delay_ms(5);
        }
        if(i == 2 || i == 3)
        {
            Arm_Motor_Motion(0x2AA, Arm_Format_Send_Data(Motor_Data[2].Position), Arm_Format_Send_Data(Motor_Data[2].Speed), Arm_Format_Send_Data(Motor_Data[3].Position), Arm_Format_Send_Data(Motor_Data[3].Speed));
            delay_ms(5);
        }
        if(i == 4 || i == 5)
        {
            Arm_Motor_Motion(0x3AA, Arm_Format_Send_Data(Motor_Data[4].Position), Arm_Format_Send_Data(Motor_Data[4].Speed), Arm_Format_Send_Data(Motor_Data[5].Position), Arm_Format_Send_Data(Motor_Data[5].Speed));
            delay_ms(5);
        }
    }
}

/******************************************************************
  * @ 函数名  ： Arm_Motor_Motion
  * @ 功能说明： 电机运动控制
  * @ 参数    ： NULL
  * @ 返回值  ： NULL
  *****************************************************************/
void Arm_Motor_Motion(int StdId, int16_t Data1, int16_t Data2, int16_t Data3, int16_t Data4)
{
    Can_Send(StdId, Data1, Data2, Data3, Data4);
}

/******************************************************************
  * @ 函数名  ： Motor_Start_Up
  * @ 功能说明： 初始启动业务
  * @ 参数    ： NULL
  * @ 返回值  ： NULL
  *****************************************************************/
void Motor_Start_Up(void)
{
//    delay_ms(500);
//    // 六轴回中
//    Arm_Motor_Motion(0x3AA, Arm_Format_Send_Data(0), Arm_Format_Send_Data(0), Arm_Format_Send_Data(0), Arm_Format_Send_Data(6000));
//    
//    delay_ms(2000);
//    // 六轴回中，五轴抬起至-250度
//    Arm_Motor_Motion(0x3AA, Arm_Format_Send_Data(-250), Arm_Format_Send_Data(10000), Arm_Format_Send_Data(0), Arm_Format_Send_Data(4000));
//    
//    delay_ms(2000);
//    // 六轴回中，五轴抬起至-50度
//    Arm_Motor_Motion(0x3AA, Arm_Format_Send_Data(-500), Arm_Format_Send_Data(5000), Arm_Format_Send_Data(0), Arm_Format_Send_Data(4000));
//    
//    delay_ms(3000);
//    // 三轴回中，四轴抬起至15度
//    Arm_Motor_Motion(0x2AA, Arm_Format_Send_Data(0), Arm_Format_Send_Data(1000), Arm_Format_Send_Data(-150), Arm_Format_Send_Data(4000));
//    delay_ms(3000);
//    // 三轴转至30度，四轴抬起至15度
//    Arm_Motor_Motion(0x2AA, Arm_Format_Send_Data(300), Arm_Format_Send_Data(3000), Arm_Format_Send_Data(-150), Arm_Format_Send_Data(4000));
//    delay_ms(4000);
//    // 三轴回中，四轴抬起至15度
//    Arm_Motor_Motion(0x2AA, Arm_Format_Send_Data(0), Arm_Format_Send_Data(3000), Arm_Format_Send_Data(-150), Arm_Format_Send_Data(4000));
//    delay_ms(3000);
//    // 三轴转至-30度，四轴抬起至15度
//    Arm_Motor_Motion(0x2AA, Arm_Format_Send_Data(-300), Arm_Format_Send_Data(3000), Arm_Format_Send_Data(-150), Arm_Format_Send_Data(4000));
//    delay_ms(3000);
//    // 三轴回中，四轴抬起至15度
//    Arm_Motor_Motion(0x2AA, Arm_Format_Send_Data(0), Arm_Format_Send_Data(3000), Arm_Format_Send_Data(-150), Arm_Format_Send_Data(4000));
//    
//    delay_ms(3000);
//    // 五轴抬起至-50度，六轴转至90度
//    Arm_Motor_Motion(0x3AA, Arm_Format_Send_Data(-500), Arm_Format_Send_Data(4000), Arm_Format_Send_Data(900), Arm_Format_Send_Data(4000));
//    delay_ms(2000);
//    // 五轴抬起至-50度，六轴转至0度
//    Arm_Motor_Motion(0x3AA, Arm_Format_Send_Data(-500), Arm_Format_Send_Data(4000), Arm_Format_Send_Data(0), Arm_Format_Send_Data(4000));
//    delay_ms(2000);
//    // 五轴抬起至-50度，六轴转至-90度
//    Arm_Motor_Motion(0x3AA, Arm_Format_Send_Data(-500), Arm_Format_Send_Data(4000), Arm_Format_Send_Data(-900), Arm_Format_Send_Data(4000));
//    delay_ms(2000);
//    // 五轴抬起至-50度，六轴转至0度
//    Arm_Motor_Motion(0x3AA, Arm_Format_Send_Data(-500), Arm_Format_Send_Data(4000), Arm_Format_Send_Data(0), Arm_Format_Send_Data(4000));
//    
//    delay_ms(3000);
//    // 二轴抬起至-30度
//    Arm_Motor_Motion(0x1AA, Arm_Format_Send_Data(0), Arm_Format_Send_Data(0), Arm_Format_Send_Data(-300), Arm_Format_Send_Data(4000));
//    delay_ms(2000);
//    // 二轴回中
//    Arm_Motor_Motion(0x1AA, Arm_Format_Send_Data(0), Arm_Format_Send_Data(0), Arm_Format_Send_Data(0), Arm_Format_Send_Data(4000));

//    delay_ms(3000);
//    // 四轴回中
//    Arm_Motor_Motion(0x2AA, Arm_Format_Send_Data(0), Arm_Format_Send_Data(4000), Arm_Format_Send_Data(0), Arm_Format_Send_Data(1000));
    
//    delay_ms(2000);
//    // 六轴回中，五轴抬起至-30度
//    Arm_Motor_Motion(0x3AA, Arm_Format_Send_Data(-300), Arm_Format_Send_Data(2000), Arm_Format_Send_Data(0), Arm_Format_Send_Data(4000));
//    delay_ms(1000);
//    // 六轴回中，五轴抬起至-10度
//    Arm_Motor_Motion(0x3AA, Arm_Format_Send_Data(-100), Arm_Format_Send_Data(2000), Arm_Format_Send_Data(0), Arm_Format_Send_Data(4000));
//    delay_ms(500);
//    // 六轴回中，五轴回中
//    Arm_Motor_Motion(0x3AA, Arm_Format_Send_Data(0), Arm_Format_Send_Data(1000), Arm_Format_Send_Data(0), Arm_Format_Send_Data(1000));
}

/******************************************************************
  * @ 函数名  ： Motor_Calibration_Motion
  * @ 功能说明： 电机标定运动控制
  * @ 参数    ： NULL
  * @ 返回值  ： NULL
  *****************************************************************/
void Motor_Calibration_Motion(int action) 
{
    if(action == 0)
    {
        Motor_Data[0].Position = 0;
        Motor_Data[0].Speed = 10000;
        Motor_Data[1].Position = 0;
        Motor_Data[1].Speed = 10000;
        Motor_Data[2].Position = 0;
        Motor_Data[2].Speed = 10000;
        Motor_Data[3].Position = 0;
        Motor_Data[3].Speed = 10000;
        Motor_Data[4].Position = 0;
        Motor_Data[4].Speed = 10000;
        Motor_Data[5].Position = 0;
        Motor_Data[5].Speed = 10000;
        Motor_Calibration_Status = 1;
        delay_ms(500);
    }
    if(action == 1)
    {
        // 五轴抬起至-250位置
        Motor_Data[4].Speed = 10000;
        Motor_Data[4].Position = -250;
        Motor_Calibration_Status = 2;
        delay_ms(3000);
    }
    
    if(action == 2)
    {
        // 五轴抬起至-350位置
        Motor_Data[4].Speed = 5000;
        Motor_Data[4].Position = -500;
        Motor_Calibration_Status = 3;
        delay_ms(3000);
    }
    
    if(action == 3)
    {
        // 四轴抬起至-220位置
        Motor_Data[3].Speed = 5000;
        Motor_Data[3].Position = -220;
        Motor_Calibration_Status = 4;
        delay_ms(3000);
    }
    
    if(action == 4)
    {
        // 三轴转至-200位置
        Motor_Data[2].Speed = 5000;
        Motor_Data[2].Position = -300;
        Motor_Calibration_Status = 5;
        delay_ms(3000);
    }
    
    if(action == 5)
    {
        // 三轴转至0位置
        Motor_Data[2].Speed = 5000;
        Motor_Data[2].Position = 0;
        Motor_Calibration_Status = 6;
        delay_ms(3000);
    }
    
    if(action == 6)
    {
        // 三轴转至300位置
        Motor_Data[2].Speed = 5000;
        Motor_Data[2].Position = 300;
        Motor_Calibration_Status = 7;
        delay_ms(3000);
    }
    
    if(action == 7)
    {
        // 三轴转至0位置
        Motor_Data[2].Speed = 5000;
        Motor_Data[2].Position = 0;
        Motor_Calibration_Status = 8;
        delay_ms(4000);
    }
    
    if(action == 8)
    {
        // 六轴转至900位置
        Motor_Data[5].Speed = 5000;
        Motor_Data[5].Position = 900;
        Motor_Calibration_Status = 9;
        delay_ms(5000);
    }
    
    if(action == 9)
    {
        // 六轴转至0位置
        Motor_Data[5].Speed = 5000;
        Motor_Data[5].Position = 0;
        Motor_Calibration_Status = 10;
        delay_ms(5000);
    }
    
    if(action == 10)
    {
        // 六轴转至-900位置
        Motor_Data[5].Speed = 5000;
        Motor_Data[5].Position = -900;
        Motor_Calibration_Status = 11;
        delay_ms(5000);
    }
    
    if(action == 11)
    {
        // 六轴转至0位置
        Motor_Data[5].Speed = 5000;
        Motor_Data[5].Position = 0;
        Motor_Calibration_Status = 12;
        delay_ms(5000);
    }
    
    if(action == 12)
    {
        // 二轴抬起至-280位置
        Motor_Data[1].Speed = 5000;
        Motor_Data[1].Position = -280;
        Motor_Calibration_Status = 13;
        delay_ms(3000);
    }
    
    if(action == 13)
    {
        Motor_Data[4].Speed = 20000;
        Motor_Data[3].Speed = 20000;
        Motor_Data[2].Speed = 20000;
        Motor_Data[1].Speed = 20000;
        Motor_Calibration_Status = 14;
        delay_ms(1000);
    }
    
}

/******************************************************************
.
  * @ 函数名  ： Arm_Task
  * @ 功能说明： 机械臂任务
  * @ 参数    ： NULL
  * @ 返回值  ： NULL
  *****************************************************************/
void Arm_Task(void)
{
    
    // 如果电机完成上电，则执行相关业务
    if(Motor_Status == 1)
    {
        //Motor_Calibration_Motion(Motor_Calibration_Status);
        //Arm_Motor_Motion_Format();
    }

    delay_ms(5);
}

/*******************************************************************************
 * @funtion      : Arm_Usb_Callback
 * @description  : 串口任务回调函数
 * @param         {char *type} 通讯协议类型
 * @return        {*}
 *******************************************************************************/
void Arm_Usb_Callback(char *type)
{
   
}
